using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Net;
using System.Collections.Generic;
using System.ComponentModel;
using System.Security.Permissions;

using Microsoft.Dss.Core.DsspHttpUtilities;
using System.Collections.Specialized;

using Microsoft.Robotics.PhysicalModel.Proxy;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
using vex = VEXRobotics.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using cons = Microsoft.Dss.Services.Constructor;
using W3C.Soap;
using robotics = Microsoft.Robotics;

/* Notes on VEX Drive
 *   Identifier must be the port number on the VEX. Range: 1 - 8.
 */
namespace VEXService.Drive
{

    /// <summary>
    /// VEX Drive State
    /// </summary>
    [DataContract, Description("Specifies the Drive state which controls two motors in synchronization.")]
    public class VEXDriveState : IDssSerializable, ICloneable
    {
        public VEXDriveState()
        {
            LeftWheel = new VEXWheelConfiguration();
            RightWheel = new VEXWheelConfiguration();
        }

        /// <summary>
        /// Name of the drive.
        /// </summary>
        [DataMember, Description("Name of the drive system.")]
        public string Name;
        
        /// <summary>
        /// Is the Drive connected to the VEX?
        /// </summary>
        [DataMember(IsRequired = false), Description("Indicates whether the drive is enabled.")]
        public bool IsEnabled;

        /// <summary>
        /// Indicates the distance between the drive wheels (meters).
        /// </summary>
        [DataMember, Description("Indicates the distance between the drive wheels in meters. \n(Example: 11.3cm = 0.113)")]
        public double DistanceBetweenWheels;

        /// <summary>
        /// Left Wheel Configuration
        /// </summary>
        [DataMember, Description("Specifies the Left Wheel Configuration.")]
        public VEXWheelConfiguration LeftWheel;

        /// <summary>
        /// Left Wheel Power
        /// </summary>
        [DataMember, Description("Specifies the Left Wheel power.")]
        public double LeftWheelPower;


        /// <summary>
        /// Right Wheel Configuration
        /// </summary>
        [DataMember, Description("Specifies the Right Wheel Configuration.")]
        public VEXWheelConfiguration RightWheel;

        /// <summary>
        /// Right Wheel Power
        /// </summary>
        [DataMember, Description("Specifies the Right Wheel power.")]
        public double RightWheelPower;

        /// <summary>
        /// Indicates the timestamp of the last state change.
        /// </summary>
        [DataMember(XmlOmitDefaultValue = true), Description("Indicates the time of the last encoder readings.")]
        [Browsable(false)]
        [DefaultValue(typeof(DateTime), "0001-01-01T00:00:00")]
        public DateTime TimeStamp;


        /// <summary>
        /// Copy state to generic state.
        /// </summary>
        /// <param name="genericState"></param>
        public drive.DriveDifferentialTwoWheelState ToGenericState(drive.DriveDifferentialTwoWheelState genericState)
        {
            genericState.DistanceBetweenWheels = this.DistanceBetweenWheels;
            genericState.IsEnabled = this.LeftWheel.MotorPort != vex.MotorPort.MotorPort_None && this.RightWheel.MotorPort != vex.MotorPort.MotorPort_None;
            genericState.TimeStamp = this.TimeStamp;

            if (genericState.LeftWheel == null)
                genericState.LeftWheel = new motor.WheeledMotorState();
            if (genericState.LeftWheel.MotorState == null)
                genericState.LeftWheel.MotorState = new motor.MotorState();
            genericState.LeftWheel.MotorState.CurrentPower = this.LeftWheelPower;
            genericState.LeftWheel.MotorState.HardwareIdentifier = (int) this.LeftWheel.MotorPort;
            genericState.LeftWheel.MotorState.PowerScalingFactor = 1.0;
            genericState.LeftWheel.MotorState.ReversePolarity = this.LeftWheel.ReversePolarity;
            genericState.LeftWheel.Radius = this.LeftWheel.WheelRadius;
            genericState.LeftWheel.GearRatio = this.LeftWheel.GearRatio;

            if (genericState.RightWheel == null)
                genericState.RightWheel = new motor.WheeledMotorState();
            if (genericState.RightWheel.MotorState == null)
                genericState.RightWheel.MotorState = new motor.MotorState();
            genericState.RightWheel.MotorState.CurrentPower = this.RightWheelPower;
            genericState.RightWheel.MotorState.HardwareIdentifier = (int) this.RightWheel.MotorPort;
            genericState.RightWheel.MotorState.PowerScalingFactor = 1.0;
            genericState.RightWheel.MotorState.ReversePolarity = this.RightWheel.ReversePolarity;
            genericState.RightWheel.Radius = this.RightWheel.WheelRadius;
            genericState.RightWheel.GearRatio = this.RightWheel.GearRatio;

            return genericState;

        }

        /// <summary>
        /// Transform generic state to this instance.
        /// </summary>
        /// <param name="genericState"></param>
        public void FromGenericState(drive.DriveDifferentialTwoWheelState genericState)
        {
            if (genericState.DistanceBetweenWheels > 0.0)
                this.DistanceBetweenWheels = genericState.DistanceBetweenWheels;

            this.TimeStamp = genericState.TimeStamp;

            if (genericState.LeftWheel != null)
            {
                if (genericState.LeftWheel.Radius > 0.0)
                    this.LeftWheel.WheelRadius = genericState.LeftWheel.Radius;

                if (genericState.LeftWheel.MotorState != null)
                {
                    this.LeftWheelPower = genericState.LeftWheel.MotorState.CurrentPower;
                    this.LeftWheel.ReversePolarity = genericState.LeftWheel.MotorState.ReversePolarity;
                }
            }

            if (genericState.RightWheel != null)
            {
                if (genericState.RightWheel.Radius > 0.0)
                    this.RightWheel.WheelRadius = genericState.RightWheel.Radius;

                if (genericState.RightWheel.MotorState != null)
                {
                    this.RightWheelPower = genericState.RightWheel.MotorState.CurrentPower;
                    this.RightWheel.ReversePolarity = genericState.RightWheel.MotorState.ReversePolarity;
                }
            }
        }




        #region IDssSerializable

        /// <summary>
        /// Copy To Analog Input sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXDriveState typedTarget = target as VEXDriveState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.DistanceBetweenWheels = this.DistanceBetweenWheels;
            typedTarget.LeftWheel = (this.LeftWheel == null) ? null : (VEXWheelConfiguration)this.LeftWheel.Clone();
            typedTarget.RightWheel = (this.RightWheel == null) ? null : (VEXWheelConfiguration)this.RightWheel.Clone();
            typedTarget.TimeStamp = this.TimeStamp;
            typedTarget.LeftWheelPower = this.LeftWheelPower;
            typedTarget.RightWheelPower = this.RightWheelPower;
            typedTarget.IsEnabled = this.IsEnabled;
        }

        /// <summary>
        /// Deep Clone DriveState
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            VEXDriveState clone = new VEXDriveState();
            clone.DistanceBetweenWheels = this.DistanceBetweenWheels;
            clone.LeftWheel = (this.LeftWheel == null) ? null : (VEXWheelConfiguration)this.LeftWheel.Clone();
            clone.RightWheel = (this.RightWheel == null) ? null : (VEXWheelConfiguration)this.RightWheel.Clone();
            clone.TimeStamp = this.TimeStamp;
            clone.LeftWheelPower = this.LeftWheelPower;
            clone.RightWheelPower = this.RightWheelPower;
            clone.IsEnabled = this.IsEnabled;
            return clone;
        }

        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(this.DistanceBetweenWheels);
            this.LeftWheel.Serialize(writer);
            this.RightWheel.Serialize(writer);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
            writer.Write(this.LeftWheelPower);
            writer.Write(this.RightWheelPower);
            writer.Write(this.IsEnabled);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            this.DistanceBetweenWheels = reader.ReadDouble();
            this.LeftWheel = new VEXWheelConfiguration();
            this.LeftWheel.Deserialize(reader);
            this.RightWheel = new VEXWheelConfiguration();
            this.RightWheel.Deserialize(reader);
            this.TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);
            this.LeftWheelPower = reader.ReadDouble();
            this.RightWheelPower = reader.ReadDouble();
            this.IsEnabled = reader.ReadBoolean();

            return this;
        }
        #endregion
    }

    /// <summary>
    /// VEX Wheel Configuration
    /// </summary>
    [DataContract, Description("Specifies the VEX Wheel Configuration.")]
    public class VEXWheelConfiguration : IDssSerializable, ICloneable
    {
        #region Constructors
        /// <summary>
        /// VEX Wheel Configuration
        /// </summary>
        public VEXWheelConfiguration()
        {
        }

        /// <summary>
        /// VEX Wheel Configuration
        /// </summary>
        /// <param name="motorPort"></param>
        /// <param name="reversePolarity"></param>
        /// <param name="wheelRadius"></param>
        public VEXWheelConfiguration(vex.MotorPort motorPort, bool reversePolarity, double wheelRadius, double gearRatio)
        {
            this.MotorPort = motorPort;
            this.ReversePolarity = reversePolarity;
            this.WheelRadius = wheelRadius;
            this.GearRatio = gearRatio;
        }
        #endregion

        /// <summary>
        /// Motor Port Configuration
        /// </summary>
        [DataMember]
        [Description("Specifies the Motor Port Configuration.")]
        public vex.MotorPort MotorPort;

        /// <summary>
        /// Reverse Motor Polarity
        /// </summary>
        [DataMember]
        [Description("Indicates the direction (polarity) of the motor.\n(Enabling this option (true) reverses the motor.)")]
        public bool ReversePolarity;

        /// <summary>
        /// Diameter of the wheel (meters)
        /// </summary>
        [DataMember]
        [Description("Specifies the radius of the wheel in meters \n(Example 2.6cm = 0.026)")]
        public double WheelRadius;

        /// <summary>
        /// Gear ratio
        /// </summary>
        [DataMember]
        [Description("Specifies the gear ratio")]
        public double GearRatio;
        #region ICloneable Members

        /// <summary>
        /// Deep Clone WheelConfiguration
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            VEXWheelConfiguration clone = new VEXWheelConfiguration(this.MotorPort, this.ReversePolarity, this.WheelRadius, this.GearRatio);
            return clone;
        }

        #endregion

        public void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(this.GearRatio);
            writer.Write((int) this.MotorPort);
            writer.Write(this.ReversePolarity);
            writer.Write(this.WheelRadius);
        }

        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            VEXWheelConfiguration newInst = new VEXWheelConfiguration();
            newInst.GearRatio = reader.ReadDouble();
            newInst.MotorPort = (vex.MotorPort)reader.ReadInt32();
            newInst.ReversePolarity = reader.ReadBoolean();
            newInst.WheelRadius = reader.ReadDouble();

            return newInst;
        }

        /// <summary>
        /// Copy To Analog Input sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXWheelConfiguration typedTarget = target as VEXWheelConfiguration;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.GearRatio = this.GearRatio;
            typedTarget.MotorPort = (vex.MotorPort)this.MotorPort;
            typedTarget.ReversePolarity = this.ReversePolarity;
            typedTarget.WheelRadius = this.WheelRadius;
        }
    }


    /// <summary>
    /// Get the VEX State
    /// </summary>
    [Description("Gets the current state of the VEX Drive.")]
    public class Get : Get<GetRequestType, PortSet<VEXDriveState, Fault>>
    {
    }

    [ServicePort]
    public class VEXDriveOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get,
        HttpGet,
        HttpPost,
        drive.ReliableSubscribe,
        drive.Subscribe,
        drive.Update,
        drive.EnableDrive,
        drive.SetDrivePower,
        drive.SetDriveSpeed,
        drive.RotateDegrees,
        drive.DriveDistance,
        drive.ResetEncoders,
        drive.AllStop>
    {
    }

    /// <summary>
    /// VEX Drive Service
    /// </summary>
    [Contract(Contract.Identifier)]
    [AlternateContract(drive.Contract.Identifier)]
    [DisplayName("VEX Drive")]
    [Description("Controls the VEX motor configuration as a two-wheel differential drive.")]
    [DssServiceDescription("http://msdn.microsoft.com/en-us/library/dd145254.aspx")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXDriveService : DsspServiceBase
    {
        /// <summary>
        /// XSLT Transform to use for display
        /// </summary>
        [EmbeddedResource("VEXService.Config.Drive.xslt")]
        private string _transform = null;

        /// <summary>
        /// The VEX Drive state
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + @"/VEX.Drive.Config.xml")]
        VEXDriveState _State = null;

        /// <summary>
        /// Service port
        /// </summary>
        [ServicePort("/VEX/Drive", AllowMultipleInstances = true)]
        private VEXDriveOperations _mainPort = new VEXDriveOperations();

        /// <summary>
        /// Alternate port for the generic port
        /// </summary>
        [AlternateServicePort("/drive",
            AllowMultipleInstances = true,
            AlternateContract = drive.Contract.Identifier)]
        private drive.DriveOperations _genericPort = new drive.DriveOperations();

        /// <summary>
        /// HTTP helper
        /// </summary>
        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();

        /// <summary>
        /// Port to talk with the VEX controller
        /// </summary>
        [Partner("VEX", Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();

        /// <summary>
        /// Subscription manager
        /// </summary>
        [Partner("SubMgr", Contract = submgr.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.CreateAlways,
            Optional = false)]
        submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="creationPort"></param>
        public VEXDriveService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }

        #region Initialization Code

        /// <summary>
        /// Startup
        /// </summary>
        protected override void Start()
        {
            LogVerbose(LogGroups.Console, "Configuring VEX Drive... ");

            #region Initialize Service State
            if (_State == null)
            {
                CreateDefaultState();
                SaveState(_State);
            }
            #endregion

            // Make sure the controller is properly configured
            VEXRobotics.Proxy.MotorConfigData leftMotorConfigData = new VEXRobotics.Proxy.MotorConfigData();
            leftMotorConfigData.Motor = _State.LeftWheel.MotorPort;
            leftMotorConfigData.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure leftConfig = new VEXRobotics.Proxy.SelectiveMotorConfigure(leftMotorConfigData);
            _vexPort.Post(leftConfig);
            VEXRobotics.Proxy.MotorConfigData rightMotorConfigData = new VEXRobotics.Proxy.MotorConfigData();
            rightMotorConfigData.Motor = _State.RightWheel.MotorPort;
            rightMotorConfigData.Type = "Continuous";
            VEXRobotics.Proxy.SelectiveMotorConfigure rightConfig = new VEXRobotics.Proxy.SelectiveMotorConfigure(rightMotorConfigData);
            _vexPort.Post(rightConfig);

            // Listen for each of my handlers
            ActivateDsspOperationHandlers();

            // Publish the service to the local Node Directory
            DirectoryInsert();

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);

            // display HTTP service Uri
            LogInfo(LogGroups.Console, "Service uri: ");
        }

        /// <summary>
        /// Set up the standard Tri-bot configuration
        /// </summary>
        void CreateDefaultState()
        {
            _State = new VEXDriveState();

            //initialize motor platform specific to VEX
            _State.DistanceBetweenWheels = 0.115;
            _State.TimeStamp = DateTime.Now;
            _State.IsEnabled = false;

            _State.LeftWheelPower = 0.0;
            _State.LeftWheel = new VEXWheelConfiguration();
            _State.LeftWheel.WheelRadius = 2.75 / 39.25;
            _State.LeftWheel.GearRatio = 1.0;
            /* FIX ME
            _State.LeftWheel.MotorState = new motor.MotorState("Left Motor", 3, 1.0, false, new robotics.PhysicalModel.Proxy.Pose());
            _State.LeftWheel.EncoderState = new robotics.Services.Encoder.Proxy.EncoderState(_State.LeftWheel.MotorState.HardwareIdentifier, 90);
             */

            _State.RightWheelPower = 0.0;
            _State.RightWheel = new VEXWheelConfiguration();
            _State.RightWheel.WheelRadius = 2.75 / 39.25;
            _State.RightWheel.GearRatio = 1.0;
            /* FIX ME
            _State.RightWheel.MotorState = new motor.MotorState("Right Motor", 2, 1.0, true, new robotics.PhysicalModel.Proxy.Pose());
            _State.RightWheel.EncoderState = new robotics.Services.Encoder.Proxy.EncoderState(_State.RightWheel.MotorState.HardwareIdentifier, 90);
             */
        }
        #endregion

        #region Standard Operation Handlers

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_State);
            yield break;
        }

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> GetHandler(drive.Get get)
        {
            drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
            get.ResponsePort.Post(_State.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Get the state to HTTP display
        /// </summary>
        /// <param name="httpGet">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _State,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// Update Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> UpdateHandler(drive.Update update)
        {
            _State.FromGenericState(update.Body);

            SaveState(_State);
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// Subscribe to updates
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> SubscribeHandler(drive.Subscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
                    SendNotification(_subMgrPort, subscribe.Body.Subscriber, new drive.Update(_State.ToGenericState(genericState)));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }

        /// <summary>
        /// ReliableSubscribe Handler
        /// </summary>
        /// <param name="subscribe"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReliableSubscribeHandler(drive.ReliableSubscribe subscribe)
        {
            yield return Arbiter.Choice(
                SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
                delegate(SuccessResult success)
                {
                    drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
                    SendNotification(_subMgrPort, subscribe.Body.Subscriber, new drive.Update(_State.ToGenericState(genericState)));
                },
                delegate(Exception fault)
                {
                    LogError(fault);
                }
            );
        }

        #endregion

        /// <summary>
        /// EnableDrive Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> EnableDriveHandler(drive.EnableDrive update)
        {
            _State.IsEnabled = update.Body.Enable;
            _State.TimeStamp = DateTime.Now;
            drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
            SendNotification(_subMgrPort, new drive.Update(_State.ToGenericState(genericState)));
            SendNotification<drive.EnableDrive>(_subMgrPort, genericState);
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// SetDrivePower Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> SetDrivePowerHandler(drive.SetDrivePower update)
        {
            _State.LeftWheelPower = Math.Min(1.0, Math.Max(-1.0, update.Body.LeftWheelPower));
            _State.RightWheelPower = Math.Min(1.0, Math.Max(-1.0, update.Body.RightWheelPower));
            if (!_State.IsEnabled)
            {
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
                yield break;
            }

            // Send left motor command
            vex.SetMotor leftMotor = new vex.SetMotor();
            leftMotor.Motor = (vex.MotorPort)_State.LeftWheel.MotorPort;
            leftMotor.Power = (int)(_State.LeftWheelPower 
                               * 127.0);
            if (_State.LeftWheel.ReversePolarity)
            {
                leftMotor.Power = (-1) * leftMotor.Power;
            }
            leftMotor.Power += 127;
            if (leftMotor.Power > 255)
                leftMotor.Power = 255;
            if (leftMotor.Power < 0)
                leftMotor.Power = 0;

            // Send right motor command
            vex.SetMotor rightMotor = new vex.SetMotor();
            rightMotor.Motor = (vex.MotorPort)_State.RightWheel.MotorPort;
            rightMotor.Power = (int)(_State.RightWheelPower
                               * 127.0);
            if (_State.RightWheel.ReversePolarity)
                rightMotor.Power = (-1) * rightMotor.Power;
            rightMotor.Power += 127;
            if (rightMotor.Power > 255)
                rightMotor.Power = 255;
            if (rightMotor.Power < 0)
                rightMotor.Power = 0;

            vex.SetMotorList motorList = new vex.SetMotorList();
            motorList.Motors = new List<vex.SetMotor>();
            motorList.Motors.Add(leftMotor);
            motorList.Motors.Add(rightMotor);
            Activate(Arbiter.Choice(_vexPort.SetMotorRequest(motorList),
                delegate(DefaultUpdateResponseType response)
                {
                },
                delegate(Fault fault)
                {
                }
            ));

            _State.TimeStamp = DateTime.Now;

            // Forward notification
            drive.DriveDifferentialTwoWheelState genericState = new drive.DriveDifferentialTwoWheelState();
            SendNotification(_subMgrPort, new drive.Update(_State.ToGenericState(genericState)));

            // Reply to sender
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
        
        /// <summary>
        /// SetDriveSpeed Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> SetDriveSpeedHandler(drive.SetDriveSpeed update)
        {
            drive.SetDrivePower dp = new drive.SetDrivePower();

            if (_State.LeftWheel.GearRatio != 0 && _State.LeftWheel.WheelRadius != 0 &&
                _State.RightWheel.GearRatio != 0 && _State.RightWheel.WheelRadius != 0)
            {
                // m/s * gearRatio / m/rev / 100rpm fs
                dp.Body.LeftWheelPower = update.Body.LeftWheelSpeed
                    * _State.LeftWheel.GearRatio
                    / (_State.LeftWheel.WheelRadius * 2.0 * System.Math.PI)
                    * 60.0;
                dp.Body.RightWheelPower = update.Body.RightWheelSpeed
                    * _State.RightWheel.GearRatio
                    / (_State.RightWheel.WheelRadius * 2.0 * System.Math.PI)
                    * 60.0;
                _genericPort.Post(dp);
                update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            }
            else
            {
                Fault fault = Fault.FromException(new NotImplementedException("Drive Speed requires gear ratio and wheel radius to be set."));
                update.ResponsePort.Post(fault);
            }
            yield break;
        }

        /// <summary>
        /// RotateDegrees Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> RotateDegreesHandler(drive.RotateDegrees update)
        {
            Fault fault = Fault.FromException(new NotImplementedException("Rotate Degrees not implemented."));
            update.ResponsePort.Post(fault);
            yield break;
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> DriveDistanceHandler(drive.DriveDistance update)
        {
            Fault fault = Fault.FromException(new NotImplementedException("Drive distance not implemented."));
            update.ResponsePort.Post(fault);
            yield break;
        }

        /// <summary>
        /// DriveDistance Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> ResetEncodersHandler(drive.ResetEncoders resetRequest)
        {
            Fault fault = Fault.FromException(new NotImplementedException("Reset encoders request not implemented."));
            resetRequest.ResponsePort.Post(fault);
            yield break;
        }

        /// <summary>
        /// AllStop Handler
        /// </summary>
        /// <param name="update"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> AllStopHandler(drive.AllStop update)
        {
            // Create a Drive request with both wheels stopped.
            // Call the SetDrivePowerHandler, but redirect the responses
            // to our response port.
            _genericPort.Post(new drive.SetDrivePower());
            update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }

        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);

            // Wait for result
            Activate(Arbiter.Choice(readForm.ResultPort,
                delegate(NameValueCollection parameters)
                {
                    if (parameters["EnableDrive"] != null)
                    {
                        drive.EnableDrive enable = new drive.EnableDrive();
                        enable.Body.Enable = true;
                        _genericPort.Post(enable);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive<DefaultUpdateResponseType>(false, enable.ResponsePort,
                                    delegate(DefaultUpdateResponseType response)
                                    {
                                        HttpPostSuccess(httpPost);
                                    }),
                                Arbiter.Receive<Fault>(false, enable.ResponsePort,
                                    delegate(Fault fault)
                                    {
                                        HttpPostFailure(httpPost, fault.Reason[0].Value);
                                    })
                            )
                        );
                    }
                    else if (parameters["DisableDrive"] != null)
                    {
                        drive.EnableDrive enable = new drive.EnableDrive();
                        enable.Body.Enable = false;
                        _genericPort.Post(enable);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive<DefaultUpdateResponseType>(false, enable.ResponsePort,
                                    delegate(DefaultUpdateResponseType response)
                                    {
                                        HttpPostSuccess(httpPost);
                                    }),
                                Arbiter.Receive<Fault>(false, enable.ResponsePort,
                                    delegate(Fault fault)
                                    {
                                        HttpPostFailure(httpPost, fault.Reason[0].Value);
                                    })
                            )
                        );
                    }
                    else if (parameters["AllStop"] != null)
                    {
                        drive.AllStop allStop = new drive.AllStop();
                        _genericPort.Post(allStop);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive<DefaultUpdateResponseType>(false, allStop.ResponsePort,
                                    delegate(DefaultUpdateResponseType response)
                                    {
                                        HttpPostSuccess(httpPost);
                                    }),
                                Arbiter.Receive<Fault>(false, allStop.ResponsePort,
                                    delegate(Fault fault)
                                    {
                                        HttpPostFailure(httpPost, fault.Reason[0].Value);
                                    })
                            )
                        );
                    }
                    else if (parameters["CustomPower"] != null)
                    {
                        if (null != parameters["Name"])
                        {
                            _State.Name = parameters["Name"];
                        }
                        double leftpower = 0;
                        double.TryParse(parameters["LeftPower"], out leftpower);
                        double rightpower = 0;
                        double.TryParse(parameters["RightPower"], out rightpower);
                        drive.SetDrivePowerRequest drivepower = new drive.SetDrivePowerRequest();
                        drivepower.LeftWheelPower = leftpower;
                        drivepower.RightWheelPower = rightpower;
                        drive.SetDrivePower setDrivePower = new drive.SetDrivePower(drivepower);
                        _genericPort.Post(setDrivePower);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive<DefaultUpdateResponseType>(false, setDrivePower.ResponsePort,
                                    delegate(DefaultUpdateResponseType response)
                                    {
                                        HttpPostSuccess(httpPost);
                                    }),
                                Arbiter.Receive<Fault>(false, setDrivePower.ResponsePort,
                                    delegate(Fault fault)
                                    {
                                        HttpPostFailure(httpPost, fault.Reason[0].Value);
                                    })
                            )
                        );
                    }
                    else
                    {
                        HttpPostFailure(httpPost, "Unknown Http Post");
                    }
                },
                delegate(Exception fault)
                {
                    LogError(fault.Message);
                })
            );
            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        /// <param name="httpPost"></param>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _State, _transform);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        /// <param name="httpPost"></param>
        /// <param name="failureReason"></param>
        private static void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.ExpectationFailed, failureReason);
            httpPost.ResponsePort.Post(rsp);
        }
    }

    /// <summary>
    /// VEXDrive Contract
    /// </summary>
    public sealed class Contract
    {
        /// The Unique Contract Identifier for the VEXDrive service
        public const String Identifier = "http://schemas.vexrobotics.com/robotics/2011/09/vexservice.drive.html";
    }
}
